/**
* @file        NaviEarthTools
* @brief       
* @version     1.0.0
* @author      Changlin.Jing 
* @date        2023/8/23 16:32
* @copyright   Copyright  2022 Langge Software Co., Ltd. All rights reserved.
*/
#include "NaviEarthTools.hpp"
namespace UtilTools::Earth{
    /**
        * 地球模型获取标准重力在N系下投影
        * @param pos
        * @param IsECEF
        * @return
        */
    Vector3d CalculateGravity(const Vector3d &pos, bool IsECEF) {
        if (IsECEF) {
            /*-------董绪荣, 张守信与华仲春, GPS/INS组合导航定位及其应用. 1998: 国防科技大学出版社.page 78-83----------------------*/
            double p = sqrt(pos(0) * pos(0) + pos(1) * pos(1) + pos(2) * pos(2));
            double t = pos(2) / p;
            double a_p = WGS84_A / p;
            double a1 = -WGS84_GM / p / p;
            double a2 = 1 + 1.5 * constant_J2 * a_p * a_p - (15.0 / 8) * constant_J4 * POW3(a_p) * a_p +
                        (35.0 / 16) * constant_J6 * POW3(a_p) * POW3(a_p);
            double a3 = -4.5 * constant_J2 * a_p * a_p + (75.0 / 4) * constant_J4 * POW3(a_p) * a_p -
                        (735.0 / 16) * constant_J6 * POW3(a_p) * POW3(a_p);
            double a4 = -(175.0 / 8) * constant_J4 * POW3(a_p) * a_p +
                        (2205.0 / 16) * constant_J6 * POW3(a_p) * POW3(a_p);
            double a5 = -(1617.0 / 16) * constant_J6 * POW3(a_p) * POW3(a_p);

            double b1 = 3 * constant_J2 * a_p * a_p - (15.0 / 2) * constant_J4 * POW3(a_p) * a_p +
                        (105.0 / 8) * constant_J6 * POW3(a_p) * POW3(a_p);
            double b2 =
                    (35.0 / 2) * constant_J4 * POW3(a_p) * a_p - (945.0 / 12) * constant_J6 * POW3(a_p) * POW3(a_p);
            double b3 = (693.0 / 8) * constant_J6 * POW3(a_p) * POW3(a_p);

            double c1 = a2;
            double c2 = a3 - b1;
            double c3 = a4 - b2;
            double c4 = a5 - b3;
            double d1 = a2 + b1;
            double d2 = c2 + b2;
            double d3 = c3 + b3;
            double d4 = c4;
            Vector3d ge_vec;
            ge_vec(0) = (c1 + c2 * t * t + c3 * POW3(t) * t + c4 * POW3(t) * POW3(t)) * pos(0) * a1 / p +
                        WGS84_AngleRate * WGS84_AngleRate * pos(0);
            ge_vec(1) = (c1 + c2 * t * t + c3 * POW3(t) * t + c4 * POW3(t) * POW3(t)) * pos(1) * a1 / p +
                        WGS84_AngleRate * WGS84_AngleRate * pos(1);
            ge_vec(2) = (d1 + d2 * t * t + d3 * POW3(t) * t + d4 * POW3(t) * POW3(t)) * pos(2) * a1 / p;
            return ge_vec;
        } else {
            double gn = 9.7803267715 * (1 + 0.0052790414 * sin(pos(0)) * sin(pos(0)) +
                                        0.0000232719 * POW3(sin(pos(0))) * sin(pos(0)));
            gn += (-0.0000030876910891 + 0.0000000043977311 * sin(pos(0)) * sin(pos(0))) * pos(2);
            gn += 0.0000000000007211 * pos(2) * pos(2);
            Vector3d gn_vec{0, 0, gn};
            return gn_vec;
        }
    }

    /**
     * @brief  BLH coordinate convert to rectangle XYZ in WGS84 coordinate
     * @note
     * @param  &BLH: [rad rad m]
     * @retval
     */
//    Eigen::Vector3d WGS84BLH2XYZ(const Eigen::Vector3d &BLH) {
//        Eigen::Vector3d XYZ{0, 0, 0};
//        double W84, N84, m_A84, m_B84, m_E84;
//
//        m_A84 = 6378137.0;
//        m_B84 = 6378137.0 * 297.257223563 / 298.257223563;
//        m_E84 = (1.0 - m_B84 * m_B84 / m_A84 / m_A84);
//        W84 = sqrt(1.00 - m_E84 * sin(BLH(0)) * sin(BLH(0)));
//        N84 = m_A84 / W84;
//
//        XYZ(0) = (N84 + BLH(2)) * cos(BLH(0)) * cos(BLH(1));
//        XYZ(1) = (N84 + BLH(2)) * cos(BLH(0)) * sin(BLH(1));
//        XYZ(2) = (N84 * (1 - m_E84) + BLH(2)) * sin(BLH(0));
//
//        return XYZ;
//    }

    /**
     * @brief  XYZ coordinate convert to BLH in WGS84 coordinate
     * @note
     * @param  &XYZ: [m,m,m]
     * @retval
     */
    Eigen::Vector3d WGS84XYZ2BLH(const Eigen::Vector3d &XYZ) {
        Eigen::Vector3d BLH;
        double a = 6378137.0;
        double b = 6378137.0 * 297.257223563 / 298.257223563;
        double e2 = (1.0 - b * b / a / a);
        double N;
        double p;
        double dtmp;
        double sinlat;
        double lat;
        double lon;
        double hgt;

        if (XYZ(0) == 0.0 && XYZ(1) == 0.0) {
            lon = 0.0;
            if (XYZ(2) < 0) {
                hgt = -XYZ(2) - b;
                lat = -M_PI / 2.0;
            } else {
                hgt = XYZ(2) - b;
                lat = M_PI / 2.0;
            }
        } else {
            p = sqrt(XYZ(0) * XYZ(0) + XYZ(1) * XYZ(1));

            lon = 2.0 * atan2(XYZ(1), (XYZ(0) + p));
            lat = atan(XYZ(2) / (p * (1.0 - e2)));
            hgt = 0.0;
            do {
                dtmp = hgt;
                sinlat = sin(lat);
                N = a / sqrt(1.0 - e2 * sinlat * sinlat);
                hgt = p / cos(lat) - N;
                lat = atan(XYZ(2) / (p * (1.0 - e2 * N / (N + hgt))));
            } while (fabs(hgt - dtmp) > 0.0001);
        }

        BLH(0) = lat;
        BLH(1) = lon;
        BLH(2) = hgt;
        return BLH;
    }

    /**
     * @brief  n系(导航坐标系)到e系(地心地固坐标系)转换矩阵
     * @note
     * @param  blh:
     * @retval
     */
    Eigen::Matrix3d cne(const Vector3d &blh) {
        double coslon, sinlon, coslat, sinlat;

        sinlat = sin(blh[0]);
        sinlon = sin(blh[1]);
        coslat = cos(blh[0]);
        coslon = cos(blh[1]);

        Eigen::Matrix3d dcm;
        dcm(0, 0) = -sinlat * coslon;
        dcm(0, 1) = -sinlon;
        dcm(0, 2) = -coslat * coslon;

        dcm(1, 0) = -sinlat * sinlon;
        dcm(1, 1) = coslon;
        dcm(1, 2) = -coslat * sinlon;

        dcm(2, 0) = coslat;
        dcm(2, 1) = 0;
        dcm(2, 2) = -sinlat;

        return dcm;
    }

    Eigen::Quaterniond blh2qne(const Eigen::Vector3d &blh) {
        Eigen::Quaterniond quat;

        double coslon, sinlon, coslat, sinlat;

        coslon = cos(blh[1] * 0.5);
        sinlon = sin(blh[1] * 0.5);
        coslat = cos(-M_PI * 0.25 - blh[0] * 0.5);
        sinlat = sin(-M_PI * 0.25 - blh[0] * 0.5);

        quat.w() = coslat * coslon;
        quat.x() = -sinlat * sinlon;
        quat.y() = sinlat * coslon;
        quat.z() = coslat * sinlon;

        return quat;
    }

    Eigen::Vector3d qne2blh(const Eigen::Quaterniond &qne, double height) {
        return {-2 * atan(qne.y() / qne.w()) - M_PI * 0.5, 2 * atan2(qne.z(), qne.w()), height};
    }

    /***
     * @brief 计算子午圈半径和卯酉圈半径
     * @param lat
     * @return
     */
    Eigen::Vector2d meridianPrimeVerticalRadius(double lat) {
        double tmp, sqrttmp;

        tmp = sin(lat);
        tmp *= tmp;
        tmp = 1 - constant::WGS84_E1 * tmp;
        sqrttmp = sqrt(tmp);

        return {constant::WGS84_A * (1 - constant::WGS84_E1) / (sqrttmp * tmp),
                constant::WGS84_A / sqrttmp};
    }

    /**
     * @brief 计算卯酉圈半径
     * @param lat
     * @return
     */
    double RN(double lat) {
        double sinlat = sin(lat);
        return constant::WGS84_A / sqrt(1.0 - constant::WGS84_E1 * sinlat * sinlat);
    }

    /**
     * @brief n系相对位置转大地坐标相对位置(位移转经纬度变化系数)
     * @param blh
     * @return
     */
    Eigen::Matrix3d DRi(const Eigen::Vector3d &blh) {
        Eigen::Matrix3d dri = Eigen::Matrix3d::Zero();

        Eigen::Vector2d rmn = meridianPrimeVerticalRadius(blh[0]);

        dri(0, 0) = 1.0 / (rmn[0] + blh[2]);
        dri(1, 1) = 1.0 / ((rmn[1] + blh[2]) * cos(blh[0]));
        dri(2, 2) = -1;
        return dri;
    }

    /**
     * 大地坐标纬经高变化量转n系相对位置 (经纬度变化转位移系数)
     * @param blh
     * @return
     */
    Eigen::Matrix3d DR(const Vector3d &blh) {
        Eigen::Matrix3d dr = Eigen::Matrix3d::Zero();

        Eigen::Vector2d rmn = meridianPrimeVerticalRadius(blh[0]);

        dr(0, 0) = rmn[0] + blh[2];
        dr(1, 1) = (rmn[1] + blh[2]) * cos(blh[0]);
        dr(2, 2) = -1;
        return dr;
    }
}